/*********************************************************
Author: Liuwei
Date: 2018.1.27
edited by huale 2018.10.270
Copyright: TCR robotics
**/
#ifndef VEHICLE_KINEMATICS_H
#define VEHICLE_KINEMATICS_H

#include <QVector>
#include "vehicledefine.h"
#include "gSoapFoundation/soapStub.h"

//struct VehicleVelocity
//{
//    double x_move;//according to vehicle unit in m/s
//    double y_move;
//    double z_rotate;//unit is rad/s
//};

//struct VehiclePosition
//{
//    double x;//m
//    double y;//m
//    double rotate;//unit is rad
//};

//using namespace hmigsoapnamespace;

/**
 * @brief The VehicleKinematics class agv运动学求解类
 */
class VehicleKinematics
{
public:
        /**
         * @brief Kinematics    Parameter initialization
         * @param wheel_diameter
         * @param base_width    The distance between two drive wheels
         */
        VehicleKinematics(E_VEHICLE_TYPE typeIn, double wheel_diameter, double base_width,
                          double base_heighth, double steeringKpIn, double wheelMaxSpeedIn, double maxSteerAdjustIn);

public:
        int resetKinematicPara(double wheel_diameter, double base_width,
                           double base_heighth);
        int setSteeringControlKp(double kpIn, double maxSteerAdjustIn);
        int setDebugFlag(int flagIn);

        /**
         * @brief setVirtualLineTrack_feedfowardOutput 设置前馈
         * @param feedforward_x
         * @param feedforward_y
         * @param feedforward_r
         * @return
         */
        int setVirtualLineTrack_feedfowardOutput(double feedforward_xIn, double feedforward_yIn
                      , double feedforward_rIn);
        /**
         * @brief setSteeringPositionFeedback
         * @param steeringPositionIn    unit is degree 用于求旋转最优解
         * @return
         */
        int setSteeringPositionFeedback(QVector<double> steeringPositionIn);
        /**
         * @brief transWheelVel2VehicleVel
         * @param wheelVel unit:degree/s
         * @param velOut m/s rad/s
         * @return
         */
        int transWheelVel2VehicleVel(QVector<double> wheelVel, QVector<double> steeringAngle, VehicleVelocity &velOut);
        /**
         * @brief transVehicleVel2WheelVel
         * @param vehicleVel m/s rad/s
         * @param wheelVel unit:degree/s
         * @return
         */
        int transVehicleVel2WheelVel(E_VEHICLE_MODE modeIn, VehicleVelocity vehicleVel,
                                     QVector<double> &steeringAngleOut,  QVector<double> &wheelVel);

private:
        /**
         * @brief getVelocitiesFromRpm
         * @param left_motor_rpm_from_encoder unit:degree/s
         * @param right_motor_rpm_from_encoder unit:degree/s
         * @param velOut m/s rad/s
         * @return
         */
        int transWheelVel2VehicleVel_two_differential(double left_wheel_vel,
                                 double right__wheel_vel, VehicleVelocity &velOut);
        /**
         * @brief getRpmFromVelocities   Because our vechile is diffrential drive robot, so linear_y speed is always 0
         * @param linear_x m/s
         * @param angular_z rad/s
         * @return wheelVel degree/s
         */
         int transVehicleVel2WheelVel_two_differential(double linear_x, double angular_z, QVector<double> &wheelVel);

         /**
          * @brief transWheelVel2VehicleVel_four_mecanum
          * @param jointVel degree/s 轮子顺序　　前右,前左，后左，后右
          * @param velOut  m/s rad/s
          * @return
          */
         int transWheelVel2VehicleVel_four_mecanum(QVector<double> jointVel, VehicleVelocity &velOut);
         int transWheelVel2VehicleVel_oneSteering(double wheelVel, double steeringAngle, VehicleVelocity &velOut);
         /**
          * @brief transVehicleVel2WheelVel_four_mecanum
          * @param linear_x m/s
          * @param angular_z rad/s
          * @param wheelVel degree/s 轮子顺序　前右,前左，后左，后右
          * @return
          */
         int transVehicleVel2WheelVel_four_mecanum(double linear_x,double linear_y, double angular_z,
                                                   QVector<double> &wheelVel);
         int transVehicleVel2WheelVel_oneSteering(double linear_x,double linear_y, double angular_z,QVector<double> &steeringAngleOut,
                                                  QVector<double> &wheelVel);

         int transVehicleVel2WheelVel_oneSteering_RotationControlOnly(
                  double linear_x, double linear_y, double angular_z, QVector<double> &steeringAngleOut,
                                                  QVector<double> &wheelVel);
         /**
          * @brief transVehicleVel2WheelVel_oneSteering_RotationControl 基于转向角度的控制
          * @param linear_x
          * @param linear_y
          * @param angular_z
          * @param steeringAngleOut
          * @param wheelVel
          * @return
          */
         int transVehicleVel2WheelVel_oneSteering_RotationControl(bool isPositiveRun, double feedAngleIn,
                  double linear_x, double linear_y, double angular_z, QVector<double> &steeringAngleOut,
                                                  QVector<double> &wheelVel);
         /**
          * @brief transVehicleVel2WheelVel_oneSteering_Gemotry 基于几何解法的控制
          * @param linear_x
          * @param linear_y
          * @param angular_z
          * @param steeringAngleOut
          * @param wheelVel
          * @return
          */
         int transVehicleVel2WheelVel_oneSteering_Gemotry(double linear_x,double linear_y, double angular_z,QVector<double> &steeringAngleOut,
                                                  QVector<double> &wheelVel);
         /**
          * @brief transVehicleVel2WheelVel_oneSteering_Feedforward 基于前馈加转角的控制。
          * @param linear_x
          * @param linear_y
          * @param angular_z
          * @param steeringAngleOut
          * @param wheelVel
          * @return
          */
         int transVehicleVel2WheelVel_oneSteering_Feedforward(double linear_x,double linear_y, double angular_z,QVector<double> &steeringAngleOut,
                                                  QVector<double> &wheelVel);

         double transformToPi(double angleIn);

    private:
        double circumference_in_m;
        double wheel_diameter_in_m;//轮子直径
        double base_width_in_m;//差动轮间距。麦克纳姆轮横向间距
        double base_heighth_in_m;//麦克纳姆轮纵向间距。单舵轮纵向间距。

        E_VEHICLE_TYPE type;
        double steeringPositionFeedback[D_STEERING_MAX_COUNT];//unit is degree 用于求旋转最优解
        double steeringKp;//转角控制比例系数
        double maxSteerAdjust;//rad

        double feedforward_x;
        double feedforward_y;
        double feedforward_r;
        int debugFlag;
        double wheelMaxSpeed;
};
#endif
